/*
 ======= [MEA] 蜂群无人机 v0.1 机体程序 概念型  by MEA群主 QQ群530683714 =========
 
 ------ 获取方块构架 -------
 采用同网格获取架构，程序遍历所有的方块，根据网格把所有方块分组，并找到自己这个编程块同网格的方块作为自己控制的方块。
 
 ----- 指令 -----
 Dock - 停泊
 Wing - 伴飞
 
 ----- 写入参数 -----
 可以通过在无人机远程控制块的自定义参数CustomData里写入一些参数：
 格式：参数名称 = 参数值，不同参数之间用换行分隔，每个参数一行。
 
 起降点设置：
 LaunchPoints = [x,y,z],[x,y,z],[x,y,z]...
 说明：等号左侧必须包含LaunchPoints关键字，等号右侧每个座标用[]封装，[]之间可以随意写，[]里的座标必须使用,分隔。这个座标是基于母船主控座椅的相对座标，x表示左-右+，y表示下-上+，z表示前-后+。注意：其中的所有符号必须是英文符号，注意输入法
 例子：LaunchPoints=[5,2,3],[0,0,2] 或者 LaunchPoints = [5,2,3] [0,0,2] [10,5,5]
*/



// ----- 方块获取 -------
static string MainCockpitNameTag = "Cockpit"; //母船主控座椅名字。必须
static string MainLCDNameTag = "FQ_LCD";//母船LCD面板名字。非必须
static string FQCockpitNameTag = "#FQ"; //无人机控制座椅或远程控制块的名字关键字。必须。只有无人机的远程控制块名字中包含这个关键字，才会被认为是一个有效的无人机。

// ----- 停泊设置 -----
static double DockDistance = 25; //停泊动作的初始距离，单位米。
static double DockSpeed = 1.5; //停泊速度，单位：米/秒。每秒减少的停泊距离。

// ----- 伴飞设置 -----
// 注意，当无人机的连接器在无人机远程控制块的前方或后方时，停泊无法校准滚转。待优化
static Vector3D WingFlyPosition = new Vector3D(0,0,0); //伴飞相对座标。在括号内填入座标，分别是X,Y,Z，其中X左-右+，Y上+下-，Z前-后+。这个座标是基于母船主控座椅为0点的   
static double WingFlyDistanceMultiple = 5; //智能伴飞点距离倍数。如果伴飞相对座标都设为0，程序自动采用智能伴飞。这是智能伴飞的距离倍数。

// ----- 瞄准控制系统设置 -----
// PID控制
static double AimRatio = 6; //瞄准精度，单位：度。用来判断炮台是否瞄准，以便其他动作判断。不影响瞄准的效率。当瞄准块的正前方向量与瞄准块和目标的连线向量夹角小于这个值时，整个系统判定瞄准了目标。
static int AimPID_T = 5; //PID 采样周期（单位：帧），周期越小效果越好，但太小的周期会让积分系数难以发挥效果
static double AimPID_P = 0.8; //比例系数：可以理解为整个PID控制的总力度，建议范围0到1.2，1是完全出力。
static double AimPID_I = 3; //积分系数：增加这个系数会让静态误差增加（即高速环绕误差），但会减少瞄准的震荡。反之同理
static double AimPID_D = 20; //微分系数：增加这个系数会减少瞄准的震荡幅度，但会加剧在小角度偏差时的震荡幅度。反之同理

// ----- 导航控制系统设置 -----
// PID控制
static double FlyRatio = 0.8; // 飞行精度，单位：米。判断是否抵达目的地。
static double FlySpeedPowerRatio = 1; // 飞船机动性系数，一定要设置好这个系数，否则导航会刹不住车。飞船机动性越好这个数越大
static int FlyPID_T = 20;// PID采样周期（单位：帧），周期越小效果越好，但太小的周期会让积分系数难以发挥效果
static double FlyPID_P = 0.8; //比例系数：可以理解为整个PID控制的总力度，建议范围0到1.2，1是完全出力。
static double FlyPID_I = 15; //积分系数：增加这个系数会让静态误差增加（即高速环绕误差），但会减少瞄准的震荡。反之同理
static double FlyPID_D = 30; //微分系数：增加这个系数会减少瞄准的震荡幅度，但会加剧在小角度偏差时的震荡幅度。反之同理

// ----- 武器子弹参数设置-----
static double BulletInitialSpeed = 400; //子弹初速度
static double BulletAcceleration = 0; //子弹加速度
static double BulletMaxSpeed = 400; //子弹最大速度
static double ShootDistance = 850; //开火距离

// ----- 其他设置 -------
static bool NeedCloseWhenDock = false; //是否在停泊和初始连接状态时开启无人机的陀螺仪和推进器，true是，false否
static bool DebugMode = false; //是否开启自检模式，该模式下程序会在编程块右下角输出错误信息


// ========== 程序正文开始 =======
static Ship MotherShip = new Ship();
static List<Ship> Ships = new List<Ship>();
static List<Target> Targets = new List<Target>();
static int t;
void Main(string arguments)
{
	if(!init){GetBlocks();return;}
	
	t ++; 
	
	//实例化母船
	MotherShip.Cockpit = MainCockpit;
	MotherShip.UpdateMotionInfo();
	
	//创建自动武器目标清单
	Targets = new List<Target>();
	foreach(Ship S in Ships){
		foreach(IMyLargeTurretBase AT in S.AutoWeapons){
			MyDetectedEntityInfo thisEntity = AT.GetTargetedEntity();
			if(!thisEntity.IsEmpty()){
				Targets.Add(new Target(thisEntity));
			}
		}
	}
	//加入母船自动武器的目标清单
	foreach(IMyLargeTurretBase AT in MainAutoWeapons){
		MyDetectedEntityInfo thisEntity = AT.GetTargetedEntity();
		if(!thisEntity.IsEmpty()){
			Targets.Add(new Target(thisEntity));
		}
	}
	//加入FCS的目标清单
	
	//飞船控制
	foreach(Ship S in Ships){
		S.MotionInit(); //初始化运动方块的越级
		S.UpdateMotionInfo(); //更新运动状态
		S.GetSetsFromCustomData();
		//执行对应指令
		switch(S.Command){
			case "Dock":
			S.Dock();
			break;
			case "Wing":
			S.Wing();
			break;
			case "Sentry":
			S.Sentry();
			break;
			case "Go":
			S.Go();
			break;
		}
	}
	
	//指令输入
	if(arguments != ""){
		foreach(Ship S in Ships){
			if(S.Command != arguments){
				S.Command = arguments;
				break;
			}
		}
	}
	
	ShowMainLCD();
}

bool init;
static IMyShipController MainCockpit;
static List<IMyLargeTurretBase> MainAutoWeapons = new List<IMyLargeTurretBase>();
static List<IMyTextPanel> MainLCDs = new List<IMyTextPanel>();
void GetBlocks()
{
	Runtime.UpdateFrequency = UpdateFrequency.Update1; //激活自触发
	
	MainCockpit = GridTerminalSystem.GetBlockWithName(MainCockpitNameTag) as IMyShipController;
	if(MainCockpit==null){Echo("Main Cockpit Not Found!");return;}
	
	GridTerminalSystem.GetBlocksOfType(MainLCDs, b => b.CustomName == MainLCDNameTag);
	GridTerminalSystem.GetBlocksOfType(MainAutoWeapons, b => b.CubeGrid == MainCockpit.CubeGrid);
	
	//获取所有方块
	List<IMyTerminalBlock> AllBlocks = new List<IMyTerminalBlock>();
	GridTerminalSystem.GetBlocksOfType(AllBlocks, b => true);
	
	//计算整个飞船包含的网格数量
	List<IMyCubeGrid> Grids = new List<IMyCubeGrid>();
	foreach(IMyTerminalBlock b in AllBlocks){
		Grids.Add(b.CubeGrid);
	}
	Grids = Grids.Distinct().ToList();
	
	//按网格传入所有方块，初始化所有网格飞船实体
	Ships = new List<Ship>();
	foreach(IMyCubeGrid grid in Grids){
		List<IMyTerminalBlock> this_gird_blocks = new List<IMyTerminalBlock>();
		foreach(IMyTerminalBlock block in AllBlocks){
			if(block.CubeGrid == grid){
				this_gird_blocks.Add(block);
			}
		}
		Ships.Add(new Ship(this_gird_blocks));
	}
	
	if(DebugMode){
		Echo("Grid Count:" + Ships.Count.ToString());
		for(int i = 0; i < Ships.Count; i ++){
			Echo(Ships[i].Debug);
		}
	}
	else{
		int SCount = Ships.Count;
		for(int i = 0; i < SCount; i ++){
			Ships.Remove(Ships.Where(p => p.Debug != "Normal").FirstOrDefault());
		}
		init = true;
	}
}

void ShowMainLCD()
{
	string info = "";
	string br = "\n";
	info += br + " ===== MEA 蜂群无人机控制系统 v0.1 =====" + br;
	info += " 无人机总数: " + Ships.Count + br;
	info += " 停泊中: " + Ships.Where(b => b.Command == "Dock").Count() + br;
	info += " 伴飞中: " + Ships.Where(b => b.Command == "Wing").Count() + br;
	info += " 目标数量: " + Targets.Count + br;
	foreach(Ship S in Ships){
		info += " " + S.Radius + br;
	}
	info += " " + MotherShip.Radius + br;
	info += " " + MotherShip.Position + br;
	foreach(IMyTextPanel L in MainLCDs){
		L.ShowPublicTextOnScreen();
		L.WritePublicText(info);
	}
}

// ============ 飞船类 ============
public class Ship
{
	// ----- 指令类变量 -----
	public string Command = "Dock";
	
	// ----- 起降控制相关 -----
	public int LaunchStep;
	public List<Vector3D> LaunchPoints = new List<Vector3D>();
	
	// ----- 运动信息和相关变量 -----
	public Vector3D Position;
	public Vector3D Velocity;
	public Vector3D Acceleration;
	public double Radius;
	
	// ----- 方块类变量 -----
	public IMyShipController Cockpit;
	public IMyRadioAntenna Antenna;
	public IMyShipConnector Connector;
	public IMyShipConnector MotherConnector;
	public Target MyAttackTarget;
	public List<IMyTerminalBlock> AllBlocks = new List<IMyTerminalBlock>();
	public List<IMyLargeTurretBase> AutoWeapons = new List<IMyLargeTurretBase>();
	public List<IMySmallGatlingGun> Guns = new List<IMySmallGatlingGun>();
	public List<IMyGyro> Gyroscopes = new List<IMyGyro>();
	public List<IMyThrust> Thrusts = new List<IMyThrust>();
	public List<string> ThrustField = new List<string>();
	public List<string> gyroYawField = new List<string>();
	public List<string> gyroPitchField = new List<string>();
	public List<string> gyroRollField = new List<string>();
	public List<float> gyroYawFactor = new List<float>();
	public List<float> gyroPitchFactor = new List<float>();
	public List<float> gyroRollFactor = new List<float>();
	public string Debug = "Normal"; //错误报告，通过这个变量判断是否初始化成功
	
	// 初始化方块方法
	public Ship(){}
	public Ship(List<IMyTerminalBlock> Blocks)
	{
		//这里面可以写入更详细的判断方块是否需要获取的条件，例如名字匹配
		AllBlocks = Blocks;
		foreach(IMyTerminalBlock b in Blocks){
			if(b is IMyShipController && b.CustomName.Contains(FQCockpitNameTag)){
				Cockpit = b as IMyShipController;
			}
			if(b is IMyRadioAntenna){
				Antenna = b as IMyRadioAntenna;
			}
			if(b is IMyLargeTurretBase){
				AutoWeapons.Add(b as IMyLargeTurretBase);
			}
			if(b is IMySmallGatlingGun){
				Guns.Add(b as IMySmallGatlingGun);
			}
			if(b is IMyGyro){
				Gyroscopes.Add(b as IMyGyro);
			}
			if(b is IMyThrust){
				Thrusts.Add(b as IMyThrust);
			}
			if(b is IMyShipConnector && (b as IMyShipConnector).OtherConnector != null){
				Connector = b as IMyShipConnector;
				MotherConnector = Connector.OtherConnector;
			}
		}
		
		if(Cockpit == null) {Debug = "Cockpit Not Found"; return;}
		if(Thrusts.Count < 1) {Debug = "Thrust Not Found";return;}
		else{
			for(int i = 0; i < Thrusts.Count; i ++)   
			{
				Base6Directions.Direction CockpitForward = Thrusts[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Forward);   
				Base6Directions.Direction CockpitUp = Thrusts[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Up);   
				Base6Directions.Direction CockpitLeft = Thrusts[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Left);   
				switch (CockpitForward)   
				{ case Base6Directions.Direction.Forward: ThrustField.Add("Forward"); break; case Base6Directions.Direction.Backward: ThrustField.Add("Backward"); break; }   
				switch (CockpitUp)   
				{ case Base6Directions.Direction.Forward: ThrustField.Add("Up"); break; case Base6Directions.Direction.Backward: ThrustField.Add("Down"); break; }   
				switch (CockpitLeft)   
				{ case Base6Directions.Direction.Forward: ThrustField.Add("Left"); break; case Base6Directions.Direction.Backward: ThrustField.Add("Right"); break; }   
			}
		}
		if(Gyroscopes.Count < 1){Debug = "Gyroscopes Not Found";return;}
		else{ 
			for (int i = 0; i < Gyroscopes.Count; i++)   
			{   
				Base6Directions.Direction gyroUp = Gyroscopes[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Up);   
				Base6Directions.Direction gyroLeft = Gyroscopes[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Left);   
				Base6Directions.Direction gyroForward = Gyroscopes[i].WorldMatrix.GetClosestDirection(Cockpit.WorldMatrix.Forward);   
				switch (gyroUp)   
				{ case Base6Directions.Direction.Up: gyroYawField.Add("Yaw"); gyroYawFactor.Add(1f); break;   
				  case Base6Directions.Direction.Down: gyroYawField.Add("Yaw"); gyroYawFactor.Add(-1f); break;   
				  case Base6Directions.Direction.Left: gyroYawField.Add("Pitch"); gyroYawFactor.Add(1f); break;   
				  case Base6Directions.Direction.Right: gyroYawField.Add("Pitch"); gyroYawFactor.Add(-1f); break;   
				  case Base6Directions.Direction.Forward: gyroYawField.Add("Roll"); gyroYawFactor.Add(-1f); break;   
				  case Base6Directions.Direction.Backward: gyroYawField.Add("Roll"); gyroYawFactor.Add(1f); break;   
				}   
				switch (gyroLeft)   
				{ case Base6Directions.Direction.Up: gyroPitchField.Add("Yaw"); gyroPitchFactor.Add(1f); break;   
				  case Base6Directions.Direction.Down: gyroPitchField.Add("Yaw"); gyroPitchFactor.Add(-1f); break;   
				  case Base6Directions.Direction.Left: gyroPitchField.Add("Pitch"); gyroPitchFactor.Add(1f); break;   
				  case Base6Directions.Direction.Right: gyroPitchField.Add("Pitch"); gyroPitchFactor.Add(-1f); break;   
				  case Base6Directions.Direction.Forward: gyroPitchField.Add("Roll"); gyroPitchFactor.Add(-1f); break;   
				  case Base6Directions.Direction.Backward: gyroPitchField.Add("Roll"); gyroPitchFactor.Add(1f); break;   
				}   
	   
				switch (gyroForward)   
				{ case Base6Directions.Direction.Up: gyroRollField.Add("Yaw"); gyroRollFactor.Add(1f); break;   
				  case Base6Directions.Direction.Down: gyroRollField.Add("Yaw"); gyroRollFactor.Add(-1f); break;   
				  case Base6Directions.Direction.Left: gyroRollField.Add("Pitch"); gyroRollFactor.Add(1f); break;   
				  case Base6Directions.Direction.Right: gyroRollField.Add("Pitch"); gyroRollFactor.Add(-1f); break;   
				  case Base6Directions.Direction.Forward: gyroRollField.Add("Roll"); gyroRollFactor.Add(-1f); break;   
				  case Base6Directions.Direction.Backward: gyroRollField.Add("Roll"); gyroRollFactor.Add(1f); break;   
				}
			}
		}
	}
	
	//从远程控制块CustoData中获取指令和相关信息
	public void GetSetsFromCustomData()
	{
		//获取起降点
		string[] Sets = this.Cockpit.CustomData.Split('\n');
		this.LaunchPoints = new List<Vector3D>();
		try{
		for(int i = 0; i < Sets.Length; i ++){
			if(Sets[i].Split('=')[0].Contains("LaunchPoints")){
				string PointsSt = Sets[i].Split('=')[1];
				for(int j = 1; j < PointsSt.Split('[').Length; j ++){
					string P_St = PointsSt.Split('[')[j].Split(']')[0];
					if(P_St.Split(',').Length >= 3){
						double X = 0;double Y = 0;double Z = 0;
						double.TryParse(P_St.Split(',')[0], out X);double.TryParse(P_St.Split(',')[1], out Y);double.TryParse(P_St.Split(',')[2], out Z);
						this.LaunchPoints.Add(new Vector3D(X,Y,Z));
					}
				}
			}
		}
		}
	}
	
	// ----- 工具类方法 -----
	//运动类方块初始化
	public void MotionInit(){
		this.SetThrustOverride("All",0);
		this.SetGyroOverride(false);
	}
	
	public void UpdateMotionInfo(){
		this.Radius = (this.Cockpit.CubeGrid.Max - this.Cockpit.CubeGrid.Min).Length() * this.Cockpit.CubeGrid.GridSize;
		this.Acceleration = ((this.Cockpit.GetPosition() - this.Position) * 60 - this.Velocity) * 60;
		this.Velocity = (this.Cockpit.GetPosition() - this.Position) * 60;
		this.Position = this.Cockpit.GetPosition();
	}
	
	//检查脱离
	public bool Detach(bool NeedDetach)
	{
		if(NeedDetach){
			this.DockStep = DockDistance;
			this.Connector.Disconnect();
			if(this.Connector.Status != MyShipConnectorStatus.Connected){
				this.TurnBlocksOnOff(this.Gyroscopes,true);
				this.TurnBlocksOnOff(this.Thrusts,true);
				this.TurnBlocksOnOff(this.AutoWeapons,true);
				//执行起飞点
				if(this.LaunchPoints.Count > 0){
					if(this.LaunchStep < 0){
						this.LaunchStep = 0;
					}
					if(this.LaunchStep > this.LaunchPoints.Count - 1){
						return true;
					}
					Vector3D LaunchPointAbs = MainCockpit.GetPosition() + MainCockpit.WorldMatrix.Right*this.LaunchPoints[this.LaunchStep].X + MainCockpit.WorldMatrix.Up*this.LaunchPoints[this.LaunchStep].Y + MainCockpit.WorldMatrix.Backward*this.LaunchPoints[this.LaunchStep].Z;
					if(FlyTo(LaunchPointAbs)){
						this.LaunchStep += 1;
					}
				}
				else{
					return true;
				}
				return false;
			}
			return false;
		}
		else{
			this.Connector.Connect();
			if(this.Connector.Status == MyShipConnectorStatus.Connected){
				this.TurnBlocksOnOff(this.Gyroscopes,NeedCloseWhenDock);
				this.TurnBlocksOnOff(this.Thrusts,NeedCloseWhenDock);
				this.TurnBlocksOnOff(this.AutoWeapons,NeedCloseWhenDock);
			}
			else{
				//执行降落点
				if(this.LaunchPoints.Count > 0){
					if(this.LaunchStep > this.LaunchPoints.Count - 1){
						this.LaunchStep = this.LaunchPoints.Count - 1;
					}
					if(this.LaunchStep < 0){
						return true;
					}
					Vector3D LaunchPointAbs = MainCockpit.GetPosition() + MainCockpit.WorldMatrix.Right*this.LaunchPoints[this.LaunchStep].X + MainCockpit.WorldMatrix.Up*this.LaunchPoints[this.LaunchStep].Y + MainCockpit.WorldMatrix.Backward*this.LaunchPoints[this.LaunchStep].Z;
					if(FlyTo(LaunchPointAbs)){
						this.LaunchStep -= 1;
					}
				}
				else{
					return true;
				}
			}
			return false;
		}
	}
	
	// ----- 任务逻辑算法 -----
	public void Go(Vector3D InputPosition = new Vector3D())
	{
		if(!this.Detach(true)){return;}//检查脱离
		Vector3D P = new Vector3D(4.26, 72.58, 14.98);
		this.FlyTo(P);
		this.CollisionAvoidance(Ships.Where( b => b != this).ToList(), MotherShip);//防撞
	}
	
	public void Sentry()
	{
		if(!this.Detach(true)){return;}//检查脱离
		
		this.Wing(false);//不瞄准的伴飞
		
		this.AttackCloestTarget(Targets, Ships.Where( b => b != this).ToList(), MotherShip);
		if(this.MyAttackTarget.EntityId == 0){
			this.Wing();
		}
		
		this.CollisionAvoidance(Ships.Where( b => b != this).ToList(), MotherShip);//防撞
	}
	
	// 伴飞控制，可选传入：是否需要指向与母船相同
	public void Wing(bool NeedAimBaseMain = true)
	{
		if(!this.Detach(true)){return;}//检查脱离
		
		Vector3D WingPoint = new Vector3D();
		if(WingFlyPosition.Length() != 0){
			WingPoint = MainCockpit.GetPosition() + MainCockpit.WorldMatrix.Right*WingFlyPosition.X + MainCockpit.WorldMatrix.Up*WingFlyPosition.Y + MainCockpit.WorldMatrix.Backward*WingFlyPosition.Z;
		}
		else{
			WingPoint = MainCockpit.GetPosition() + (this.MotherConnector.GetPosition() - MainCockpit.GetPosition())*WingFlyDistanceMultiple; 
		}
		
		//控制指向
		if(NeedAimBaseMain){
			if(this.AimAt(MainCockpit.WorldMatrix.Forward*100000000)){   
				double Y_Angle = Vector3D.Dot(MainCockpit.WorldMatrix.Up, this.Cockpit.WorldMatrix.Up);
				double X_Angle = Vector3D.Dot(MainCockpit.WorldMatrix.Left, this.Cockpit.WorldMatrix.Up);
				if(X_Angle > 0){   
					SetGyroValue(0,0,(Y_Angle+1)*60);
				}   
				else{   
					SetGyroValue(0,0,-(Y_Angle+1)*60);
				}
			}
		}
		
		//运动控制
		this.FlyTo(WingPoint,MainCockpit.GetShipVelocities().LinearVelocity);
		this.CollisionAvoidance(Ships.Where( b => b != this).ToList(), MotherShip);//防撞
	}
	
	private double DockStep; 
	public void Dock()
	{
		if(!this.Detach(false)){
			this.CollisionAvoidance(Ships.Where( b => b != this).ToList(), MotherShip);//防撞
			return;
		}
		
		//指向控制
		Base6Directions.Direction CockpitToConnectorForward = this.Cockpit.WorldMatrix.GetClosestDirection(this.Connector.WorldMatrix.Forward);
		Vector3D AimVector = MainCockpit.WorldMatrix.Forward;
		double Y_Angle = Vector3D.Dot(this.MotherConnector.WorldMatrix.Forward, this.Connector.WorldMatrix.Forward);   
		double X_Angle = Vector3D.Dot(Vector3D.Cross(this.MotherConnector.WorldMatrix.Forward, this.Connector.WorldMatrix.Forward), MainCockpit.WorldMatrix.Forward);
		//解决连接器在前后的情况
		switch(CockpitToConnectorForward){
			case Base6Directions.Direction.Forward:
			AimVector = this.MotherConnector.WorldMatrix.Backward;
			break;
			case Base6Directions.Direction.Backward:
			AimVector = this.MotherConnector.WorldMatrix.Forward;
			break;
		}
		if(this.AimAt(AimVector*100000000)){
			if(X_Angle < 0){   
				SetGyroValue(0,0,(Y_Angle+1)*60);
			}   
			else{   
				SetGyroValue(0,0,-(Y_Angle+1)*60);
			} 
		}   
		   
			//计算座标   
		Vector3D MeConnectorToCockpitVector =  this.Cockpit.GetPosition() - this.Connector.GetPosition();   
		Vector3D DockPoint = MotherConnector.GetPosition() + MotherConnector.WorldMatrix.Forward*DockStep + MeConnectorToCockpitVector;   
		if(t%60 == 0 && DockStep > 0.5){   
			DockStep -= DockSpeed;
		}   
		
		this.FlyTo(DockPoint, MainCockpit.GetShipVelocities().LinearVelocity);
	}
	
	// ----- 作战控制类 -----
	public void AttackCloestTarget(List<Target> InputTargets, List<Ship> InputShips = null, Ship InputMotherShip = null)
	{
		//检索最近的目标
		double currentDis = double.MaxValue;
		this.MyAttackTarget = new Target();
		//把母船加入列表
		if(InputShips == null){InputShips = new List<Ship>();}
		if(InputMotherShip != null){InputShips.Add(InputMotherShip);}
		
		for(int i = 0; i < InputTargets.Count; i ++){
			double distance = Vector3D.Distance(InputTargets[i].Position, this.Position);
			//排除被友军阻挡的目标
			if(InputShips.Count > 0){
				for(int j = 0; j < InputShips.Count; j ++){
					//利用航点内垂线函数计算该友军与自己和目标连线的距离，并通过友军半径判断
					bool ShipNotBetweenMeAndTarget = Vector3D.Dot(Vector3D.Normalize(InputTargets[i].Position - InputShips[j].Position), Vector3D.Normalize(this.Position - InputShips[j].Position)) >= 0;
					bool CanNotHitShip = Vector3D.Distance(RouteVerticalPoint(InputShips[j].Position, this.Position, InputTargets[i].Position), InputShips[j].Position) >= InputShips[j].Radius;
					if((ShipNotBetweenMeAndTarget || CanNotHitShip) && distance <= currentDis){
						this.MyAttackTarget = InputTargets[i];
						currentDis = distance;
					}
				}
			}
			else{
				//利用航点内垂线函数计算该友军与自己和目标连线的距离，并通过友军半径判断
				if(distance <= currentDis){
					this.MyAttackTarget = InputTargets[i];
					currentDis = distance;
				}
			}
			
		}
		this.AttackTarget(this.MyAttackTarget);
	}
	public void AttackTarget(Target InputTarget)
	{
		if(InputTarget.EntityId != 0){
			this.AttackTarget(InputTarget.Position, InputTarget.Velocity, InputTarget.Acceleration);
		}
	}
	public void AttackTarget(Vector3D InputPosition, Vector3D InputVelocity = new Vector3D(), Vector3D InputAcceleration = new Vector3D())
	{
		Vector3D HitPoint = HitPointCaculate(this.Position, this.Velocity, this.Acceleration, InputPosition,InputVelocity, InputAcceleration, BulletInitialSpeed, BulletAcceleration, BulletMaxSpeed);
		if(this.AimAt(HitPoint) && Vector3D.Distance(HitPoint, this.Position) <= ShootDistance){
			this.GunsShoot();
		}
	}
	
	// ----- 运动控制算法 -----
	// PID瞄准目标函数
	private List<Vector3D> Aim_PID_Data = new List<Vector3D>();
	public bool AimAt(Vector3D TargetPos)
	{
		MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), this.Cockpit.WorldMatrix.Forward, this.Cockpit.WorldMatrix.Up);
		Vector3D PositionToMe = Vector3D.Normalize(Vector3D.TransformNormal(TargetPos - this.Position, refLookAtMatrix));
		
		//储存采样点
		if(Aim_PID_Data.Count < AimPID_T){
			for(int i = 0; i < AimPID_T; i ++){
				Aim_PID_Data.Add(new Vector3D());
			}
		}
		else{Aim_PID_Data.Remove(Aim_PID_Data[0]); Aim_PID_Data.Add(PositionToMe);}
		
		//获得采样点积分
		double X_I = 0;
		double Y_I = 0;
		foreach(Vector3D datapoint in Aim_PID_Data){
			X_I += datapoint.X;
			Y_I += datapoint.Y;
		}
		
		//计算输出
		double YawValue = AimPID_P*(PositionToMe.X + (1/AimPID_I)*X_I + AimPID_D*(Aim_PID_Data[AimPID_T-1].X - Aim_PID_Data[0].X)/AimPID_T) * 60;
		double PitchValue = AimPID_P*(PositionToMe.Y + (1/AimPID_I)*Y_I + AimPID_D*(Aim_PID_Data[AimPID_T-1].Y - Aim_PID_Data[0].Y)/AimPID_T) * 60;
		this.SetGyroValue(YawValue, PitchValue, 0);
		this.SetGyroOverride(true);
		
		// 计算当前与预期瞄准点的瞄准夹角
		Vector3D V_A = TargetPos - this.Position;
		Vector3D V_B = this.Cockpit.WorldMatrix.Forward;
		double Angle = Math.Acos(Vector3D.Dot(V_A,V_B)/(V_A.Length() * V_B.Length())) * 180 / Math.PI;
		if(Angle <= AimRatio){return true;}
		else{return false;}
	}
	
	//飞行控制函数
	private List<Vector3D> Fly_PID_Data = new List<Vector3D>();
	public bool FlyTo(Vector3D TargetPos, Vector3D Velocity = new Vector3D(), Vector3D Acceleration = new Vector3D())
	{
		MatrixD refLookAtMatrix = MatrixD.CreateLookAt(new Vector3D(), this.Cockpit.WorldMatrix.Forward, this.Cockpit.WorldMatrix.Up);
		Vector3D PositionToMe = Vector3D.TransformNormal(TargetPos - this.Position, refLookAtMatrix);
		Vector3D VelocityToMe = Vector3D.TransformNormal(Velocity - this.Velocity, refLookAtMatrix);
		//设定一个差值
		Vector3D DataDiff = PositionToMe + VelocityToMe/FlySpeedPowerRatio;
		
		//储存采样点
		if(Fly_PID_Data.Count < FlyPID_T){
			for(int i = 0; i < FlyPID_T; i ++){
				Fly_PID_Data.Add(new Vector3D());
			}
		}
		else{Fly_PID_Data.Remove(Fly_PID_Data[0]); Fly_PID_Data.Add(DataDiff);}
		
		//获得采样点积分
		double X_I = 0;
		double Y_I = 0;
		double Z_I = 0;
		foreach(Vector3D datapoint in Fly_PID_Data){
			X_I += datapoint.X;
			Y_I += datapoint.Y;
			Z_I += datapoint.Z;
		}
		
		//计算输出
		double FB_Value = FlyPID_P*(DataDiff.Z + (1/FlyPID_I)*Z_I + FlyPID_D*(Fly_PID_Data[FlyPID_T-1].Z - Fly_PID_Data[0].Z)/FlyPID_T);
		double LR_Value = FlyPID_P*(DataDiff.X + (1/FlyPID_I)*X_I + FlyPID_D*(Fly_PID_Data[FlyPID_T-1].X - Fly_PID_Data[0].X)/FlyPID_T);
		double UD_Value = FlyPID_P*(DataDiff.Y + (1/FlyPID_I)*Y_I + FlyPID_D*(Fly_PID_Data[FlyPID_T-1].Y - Fly_PID_Data[0].Y)/FlyPID_T);
		
		if(PositionToMe.Length() >= FlyRatio){
			this.SetThrustOverride("Forward", FB_Value);
			this.SetThrustOverride("Backward", -FB_Value);
			this.SetThrustOverride("Left", LR_Value);
			this.SetThrustOverride("Right", -LR_Value);
			this.SetThrustOverride("Up", -UD_Value);
			this.SetThrustOverride("Down", UD_Value);
			return false;
		}
		else{
			return true;
		}
	}
	
	//防撞控制
	public void CollisionAvoidance(List<Ship> InputShips, Ship InputMotherShip = null)
	{
		//把母船加入列表
		if(InputMotherShip != null){
			InputShips.Add(InputMotherShip);
		}
		
		List<int> Collision_T = new List<int>();
		List<Ship> CollisionShip = new List<Ship>();
		//获得所有可能碰撞的友军飞船和碰撞时间
		foreach(Ship S in InputShips){
			//预判到3秒后，每10帧计算一次
			for(int i = 0; i <= 180; i+=10){
				Vector3D MeCenterPoint = this.Position + this.Velocity*i/60;
				Vector3D HeCenterPoint = S.Position + S.Velocity*i/60;
				if(Vector3D.Distance(MeCenterPoint,HeCenterPoint) <= this.Radius + S.Radius){
					Collision_T.Add(i);
					CollisionShip.Add(S);
					break;
				}
			}
		}
		
		//检索碰撞时间最小的友军飞船
		Ship DangerShip = null;
		int T_temp = int.MaxValue;
		for(int i = 0; i < CollisionShip.Count; i ++){
			if(Collision_T[i] <= T_temp){
				T_temp = Collision_T[i];
				DangerShip = CollisionShip[i];
			}
		}
		
		//执行避撞
		if(DangerShip != null){
			Vector3D AvoidPoint = DangerShip.Position + Vector3D.Normalize(this.Position - DangerShip.Position)*1000;
			this.FlyTo(AvoidPoint);
		}
	}
	
	// ----- 基础控制类方法 -----
	public void TurnBlocksOnOff(List<IMyTerminalBlock> B, bool o)
	{
		foreach(var b in B){
			b.ApplyAction(o?"OnOff_On":"OnOff_Off");
		}
	}
	public void TurnBlocksOnOff(List<IMyGyro> B, bool o){
		List<IMyTerminalBlock> BList = new List<IMyTerminalBlock>();
		foreach(var b in B){
			BList.Add(b as IMyTerminalBlock);
		}
		TurnBlocksOnOff(BList,o);
	}
	public void TurnBlocksOnOff(List<IMyThrust> B, bool o){
		List<IMyTerminalBlock> BList = new List<IMyTerminalBlock>();
		foreach(var b in B){
			BList.Add(b as IMyTerminalBlock);
		}
		TurnBlocksOnOff(BList,o);
	}
	public void TurnBlocksOnOff(List<IMyLargeTurretBase> B, bool o){
		List<IMyTerminalBlock> BList = new List<IMyTerminalBlock>();
		foreach(var b in B){
			BList.Add(b as IMyTerminalBlock);
		}
		TurnBlocksOnOff(BList,o);
	}
	
	public void SetThrustOverride(string Direction, double Value)   
	{
		for(int i = 0; i < this.Thrusts.Count; i ++){
			if(Direction == "All"){this.Thrusts[i].ThrustOverridePercentage = (float)Value;}
			else{if(this.ThrustField[i] == Direction){this.Thrusts[i].ThrustOverridePercentage = (float)Value;}}
		}
	}
	public void SetGyroOverride(bool bOverride)   
	{
		foreach(IMyGyro g in this.Gyroscopes){g.GyroOverride = bOverride;} 
	}
	public void SetGyroValue(double Y, double P, double R)
	{
		for (int i = 0; i < this.Gyroscopes.Count; i++){
			this.Gyroscopes[i].SetValue(gyroYawField[i], (float)Y * gyroYawFactor[i]);
			this.Gyroscopes[i].SetValue(gyroPitchField[i], (float)P * gyroPitchFactor[i]);
			this.Gyroscopes[i].SetValue(gyroRollField[i], (float)R * gyroRollFactor[i]);
		}
	}
	public void GunsShoot()
	{
		foreach(IMySmallGatlingGun g in this.Guns){g.ApplyAction("ShootOnce");}
	}
}

// ============ 目标类 ============
public class Target
{
	public string Name;
	public long EntityId;
	public double Radius;
	public int TimeStamp;
	public MyDetectedEntityType Type;
	public Vector3D Position;
	public Vector3D Velocity;
	public Vector3D Acceleration;
	public MatrixD Orientation;
	
	public Target(){
		this.EntityId = 0;
	}
	public Target(MyDetectedEntityInfo thisEntity){
		this.EntityId = thisEntity.EntityId;
		this.Name = thisEntity.Name;
		this.EntityId = thisEntity.EntityId;
		this.Radius = Vector3D.Distance(thisEntity.BoundingBox.Max, thisEntity.BoundingBox.Min);
		Vector3D.TryParse(thisEntity.Position.ToString(), out this.Position);
		Vector3D.TryParse(thisEntity.Velocity.ToString(), out this.Velocity);
		this.Orientation = thisEntity.Orientation;
		this.TimeStamp = t;
	}
}

// ============= 运算类函数 =====================   
//计算碰撞点的函数，分别传入：飞船座标、飞船速度、飞船加速度、目标座标、目标速度、目标加速度、子弹初速度、子弹加速度、子弹末速度       
static Vector3D HitPointCaculate(Vector3D Me_Position, Vector3D Me_Velocity, Vector3D Me_Acceleration, Vector3D Target_Position, Vector3D Target_Velocity, Vector3D Target_Acceleration,        
							double Bullet_InitialSpeed, double Bullet_Acceleration, double Bullet_MaxSpeed)       
{       
	//迭代算法       
	Vector3D HitPoint = new Vector3D();       
	Vector3D Smt = Target_Position - Me_Position;//发射点指向目标的矢量       
	Vector3D Velocity = Target_Velocity - Me_Velocity; //目标飞船和自己飞船总速度       
	Vector3D Acceleration = Target_Acceleration; //目标飞船和自己飞船总加速度       
	       
	double AccTime = (Bullet_Acceleration == 0 ? 0 : (Bullet_MaxSpeed - Bullet_InitialSpeed)/Bullet_Acceleration);//子弹加速到最大速度所需时间       
	double AccDistance = Bullet_InitialSpeed*AccTime + 0.5*Bullet_Acceleration*AccTime*AccTime;//子弹加速到最大速度经过的路程       
	       
	double HitTime = 0;       
	       
	if(AccDistance < Smt.Length())//目标在炮弹加速过程外       
	{       
		HitTime = (Smt.Length() - Bullet_InitialSpeed*AccTime - 0.5*Bullet_Acceleration*AccTime*AccTime + Bullet_MaxSpeed*AccTime)/Bullet_MaxSpeed;       
		HitPoint = Target_Position + Velocity*HitTime + 0.5*Acceleration*HitTime*HitTime;       
	}       
	else//目标在炮弹加速过程内     
	{       
		double HitTime_Z = (-Bullet_InitialSpeed + Math.Pow((Bullet_InitialSpeed*Bullet_InitialSpeed + 2*Bullet_Acceleration*Smt.Length()),0.5))/Bullet_Acceleration;       
		double HitTime_F = (-Bullet_InitialSpeed - Math.Pow((Bullet_InitialSpeed*Bullet_InitialSpeed + 2*Bullet_Acceleration*Smt.Length()),0.5))/Bullet_Acceleration;       
		HitTime = (HitTime_Z > 0 ? (HitTime_F > 0 ? (HitTime_Z < HitTime_F ? HitTime_Z : HitTime_F) : HitTime_Z) : HitTime_F);       
		HitPoint = Target_Position + Velocity*HitTime + 0.5*Acceleration*HitTime*HitTime;       
	}       
	//迭代，仅迭代更新碰撞时间，每次迭代更新右5位数量级       
	for(int i = 0; i < 3; i ++)       
	{       
		if(AccDistance < Vector3D.Distance(HitPoint,Me_Position))//目标在炮弹加速过程外       
		{       
			HitTime = (Vector3D.Distance(HitPoint,Me_Position) - Bullet_InitialSpeed*AccTime - 0.5*Bullet_Acceleration*AccTime*AccTime + Bullet_MaxSpeed*AccTime)/Bullet_MaxSpeed;       
			HitPoint = Target_Position + Velocity*HitTime + 0.5*Acceleration*HitTime*HitTime;       
		}       
		else//目标在炮弹加速过程内       
		{       
			double HitTime_Z = (-Bullet_InitialSpeed + Math.Pow((Bullet_InitialSpeed*Bullet_InitialSpeed + 2*Bullet_Acceleration*Vector3D.Distance(HitPoint,Me_Position)),0.5))/Bullet_Acceleration;       
			double HitTime_F = (-Bullet_InitialSpeed - Math.Pow((Bullet_InitialSpeed*Bullet_InitialSpeed + 2*Bullet_Acceleration*Vector3D.Distance(HitPoint,Me_Position)),0.5))/Bullet_Acceleration;       
			HitTime = (HitTime_Z > 0 ? (HitTime_F > 0 ? (HitTime_Z < HitTime_F ? HitTime_Z : HitTime_F) : HitTime_Z) : HitTime_F);       
			HitPoint = Target_Position + Velocity*HitTime + 0.5*Acceleration*HitTime*HitTime;       
		}       
	}       
	return HitPoint;       
}

// ---------------- 航线内垂点计算函数 ------------   
// pos1 是航线外的点，pos2和pos3是航线的两个端点   
// 本函数返回pos1与航线的垂线，与该航线的交点座标   
static Vector3D RouteVerticalPoint(Vector3D pos1, Vector3D pos2, Vector3D pos3)   
{   
	var Ratio_t = ((pos2.X - pos1.X)*(pos3.X - pos2.X) + (pos2.Y - pos1.Y)*(pos3.Y - pos2.Y) + (pos2.Z - pos1.Z)*(pos3.Z - pos2.Z))   
					/( Math.Pow(pos3.X-pos2.X,2) + Math.Pow(pos3.Y-pos2.Y,2) + Math.Pow(pos3.Z - pos2.Z,2));   
	double x = pos2.X - (pos3.X - pos2.X)*Ratio_t;   
	double y = pos2.Y - (pos3.Y - pos2.Y)*Ratio_t;   
	double z = pos2.Z - (pos3.Z - pos2.Z)*Ratio_t;   
	return new Vector3D(x,y,z);   
}   
